#!/usr/bin/python3
import logging
import os

from IPython import embed
import trimesh
import numpy as np
from path import Path
from scipy.spatial.transform import Rotation
from vispy.geometry import generation
from vispy.io import read_mesh
from vispy.scene import ViewBox, transforms, visuals
from vispy.scene.canvas import SceneCanvas

from .Utils import ToTransformVisForm
from .XmlParser import RobotTree

# logging.basicConfig(format="%(asctime)s : %(msg)s")
logger = logging.getLogger(__file__)


def LoadMesh(file_path: str, view: ViewBox, parent=None, transform=transforms.NullTransform(),
             color=(0.5, 0, 0, 0.8), show_face=True, show_edge=False):
    if not os.path.exists(file_path):
        logger.error(f"input {file_path} is not a valid mesh file")
        return
    if Path(file_path).ext == ".dae":
        meshes = []
        dae_data = trimesh.exchange.dae.load_collada(file_path)
        geo = dae_data.get('geometry')
        if geo:
            for _, mesh_data in geo.items():
                meshes.append(visuals.Mesh(
                    vertices=mesh_data['vertices'], faces=mesh_data['faces'], color=color))
        visual = visuals.Compound(meshes)
        view.add(visual)
        if not parent:
            view.add(visual)
        else:
            visual.parent = parent
        return visual
    # for stl and obj file
    m = trimesh.load_mesh(file_path)
    meshes = []
    if show_face:
        mesh = visuals.Mesh(
            vertices=m.vertices, faces=m.faces, color=color)
        meshes.append(mesh)
    if show_edge:
        mesh_edges = visuals.Line(
            m.vertices, connect=m.edges_unique, color=(0, 0, 0, 1))
        meshes.append(mesh_edges)
    logger.info(f"{file_path} has been loaded")

    visual = visuals.Compound(meshes)
    visual.transform = transform
    if not parent:
        view.add(visual)
    else:
        visual.parent = parent
    return visual


def PlotSphere(canvas: SceneCanvas, radius: float = 0.2):
    canvas.unfreeze()
    mesh = generation.create_sphere(20, 20, radius=radius)
    vertices = mesh.get_vertices()
    triangles = mesh.get_faces()
    cl = np.linspace(-radius, radius, 6 + 2)[1:-1]

    canvas.iso = visuals.Isoline(vertices=vertices, tris=triangles,
                                 data=vertices[:, 2],
                                 levels=cl, color_lev='autumn',
                                 parent=canvas.view.scene, name="IsoLine")
    canvas.radius = radius
    canvas.freeze()


def DeleteVisualNode(node: visuals.VisualNode):
    node.parent = None


def SetData(canvas, n_levels, cmap):
    canvas.iso.set_color(cmap)
    cl = np.linspace(-canvas.radius, canvas.radius, n_levels + 2)[1:-1]
    canvas.iso.levels = cl


def LoadUrdf(canvas: SceneCanvas, urdf_path: str, show_edge=False, show_face=True):
    robot_tree = RobotTree(urdf_path)
    joints = robot_tree.GetJoints()
    links = robot_tree.GetLinks()
    mesh = canvas.view.scene
    color = robot_tree.GetRGBA()
    link_meshes = {}
    for node in robot_tree.GetChain():
        link = links.get(node, False)
        if not link or link.get("visual_mesh", "") == "":
            continue
        mesh = LoadMesh(link.get('visual_mesh'),
                        view=canvas.view, parent=mesh, color=color, show_face=show_face, show_edge=show_edge)
        link_meshes[link['name']] = mesh
        if link["parent"]:
            parent_joint_pos = joints[link["parent"]]["origin_xyz"]
            parent_joint_rpy = joints[link["parent"]]["origin_rpy"]
            mesh.transform = transforms.MatrixTransform(
                ToTransformVisForm(parent_joint_pos, parent_joint_rpy))
    return robot_tree.GetName(), robot_tree, link_meshes


if __name__ == "__main__":
    cwd = os.path.dirname(__file__)
    urdf_path = Path(cwd) / ".." / "models/GP12/GP12.urdf"
    from IPython import embed
    tree = RobotTree(urdf_path.abspath())
    embed()
